/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Filters;

import Jama.Matrix;


/**
 *
 * @author Anne AND Jonathan
 */
public class Kalman3 {
    private double[][] signals;// original array of all signals, row = signal, column = output
    private double[][] corrected; // the values in here, correspond to every predicted X, calculated by the kalman filter, for each signal.

    private double noiseVariance;
    private double predictedXVariance;

    //Important: the following two variables change for every change in i or k !
    private Matrix sourceYK; //the combined big Y-i matrix, from the current k and without signal (row) i
    private Matrix sourceK; // the yk from the signal currently being corrected (signal i, dus)
    
    private Matrix mNC, pNC; // measurement/process noise covariance
    private Matrix kalmanGain;    
    private Matrix pX, pXC; // optimal bayes estimate(predicted x) and its covariance, so the predicted X/process noise    

    //the following two variables contain the predicted X and its covariance of each signal
    private Matrix[] apX, apXC; // every signal needs a separate predicted X and covariance
    
    private int sourceLength; // nr of samples in the source
    private int nrSH; // nr of samples per heartbeat (ECG complex)
    private int totalk; // nr of heartbeats per signal
    private int N = 5; // number of heartbeats to get mean for init of pX
    private final double AMPLITUDE = 0.25;
    
    
    public Kalman3(){}
    
    public double[][] init(double[][] signalsI, double time){
        signals = copyArray(signalsI);
        sourceLength = signals[0].length;
        nrSH = calculateSH(time); //calculate Samples between Heartbeats
        totalk = sourceLength/nrSH; // we lose precision -> int division < but it can't be a double!>
        
        
        /**put in here: initialization of:
         * pX = (initPX)
         * mNC en noiseVariance can be done as usual.
         * predictedXVariance = getVariance(pX)
        */
        // ORIGINAL CODE :
        // pX = initPX(signals);
        //pXC = Matrix.identity(nrSH, nrSH).times(getVariance(pX)); //in principle pXC is scalar matrix of variance of pX! we use this, since no other initializatio is specified
        /**
         * for (int i = 0;i < signals.length; i++){
            apX[i] = pX;  
            apXC[i] = pXC;        
         */

        apX = new Matrix[signals.length];
        apXC = new Matrix[signals.length];

        for (int i=0; i < signals.length; i++){
            apX[i] = initPX(signals[i]);
            apXC[i] = Matrix.identity(nrSH, nrSH).times(getVariance(apX[i]));
        }
        
        corrected = new double[signals.length][signals[0].length];
        
        calculate();
      
         for (int i=0;i<signals.length; i++){
            for (int j=0; j<corrected[0].length; j+= 100){
                //System.out.println("value of i: " + i + ", vallue of j: " + j+ " = " + corrected[i][j]);
            }
        }
        return corrected;
    }
    
    public void calculate(){
        for (int k = 0; k < totalk; k++){ // per heartbeat --> for every signal
            for (int i = 0; i < signals.length; i++){
                updateSourceK(k, i);
                updateSourceYK(k, i);
                updateMeasurementNoise();
                updateProcessNoise(i);
                updateKalmanGain(i);
                updateKalmanFilter(k, i);
            }
        }        
    }

    // updates the sourceK variable to the current signal i and current heartbeat k = y of k
    public void updateSourceK(int k, int i){
        double[] temp = new double[nrSH];
        int z = 0;
        for (int j = k*nrSH; j <k*nrSH + nrSH; j++ ){
            temp[z] = signals[i][j];
            z++;
        }
        sourceK = new Matrix(temp, 1);
    }

    // updates sourceYK to the current signal i and current heartbeat k = Y-i of k
    public void updateSourceYK(int k, int i){
        // big Y-i is without the i signal, so it's one row smaller than signals!
          double[][] temp = new double[signals.length - 1][nrSH];
          
          int p = 0;
          for (int l = 0; l< signals.length; l++){
              int z = 0;
              // if l == i, do nothing => leave out the ith row!!
              if (i != l){
                for (int j = k*nrSH; j <k*nrSH + nrSH; j++, z++ ){
                    temp[p][z] = signals[l][j]; // need p here, sinds both i and l are used in a different way! => easiest solution
                }
                p++;
              }
          }
          sourceYK = new Matrix(temp);
    }

     public void updateMeasurementNoise(){
        double tempMSE = (sourceYK.transpose().times(sourceYK)).inverse().get(0,0); // transpose*normal = double !        
        Matrix mse = matrixTimesVector(sourceYK, sourceK).times(tempMSE);
        Matrix predictedY = matrixTimesVector(sourceYK, mse);
        Matrix mNCVector = sourceK.minus(predictedY);
        noiseVariance = getVariance(mNCVector);
        mNC = getCovariance(mNCVector);
        
    }

    //remember that the source needs to be updated beforehand!!: sourceK+1 !
    public void updateProcessNoise(int i){
        pX = apX[i];
        predictedXVariance = getVariance(pX);
        Matrix modelResidual = sourceK.minus(pX);
        double normalize = 1.0/nrSH;
        double a = modelResidual.transpose().times(modelResidual).get(0, 0);
        double tempP = a*normalize - predictedXVariance - noiseVariance;
        if (tempP >= 0)
            pNC = Matrix.identity(nrSH, nrSH).times(tempP);
        else
            pNC = Matrix.identity(nrSH, nrSH).times(0);
    }

    public void updateKalmanGain(int i){
        pXC = apXC[i];
        Matrix tempE = mNC.plus(pXC.plus(pNC));
        //System.out.println(" mNC column: "+ mNC.getColumnDimension() + " row: " + mNC.getRowDimension());
        double p = tempE.det();
        if (p > 0 || p < 0){
            //System.out.println(" determinant: " + p);
            tempE = tempE.inverse();
        }
        Matrix tempK = (pXC.plus(pNC)).times(tempE);
        // kalmanGaink+1 = (pXCk + pnCk)/ (mNCk+1 + PXCk + pNCk)
        kalmanGain = tempK;
    }

    public void updateKalmanFilter(int k, int i){
        pX = apX[i];
        pXC = apXC[i];
        //System.out.println("r,c sourceK: " + sourceK.getRowDimension()+ " " + sourceK.getColumnDimension());
        //System.out.println("r,c pX: " + pX.getRowDimension()+ " " + pX.getColumnDimension());
        //System.out.println("r,c kalmanGain: " + kalmanGain.getRowDimension()+ " " + kalmanGain.getColumnDimension());
        Matrix vector = sourceK.minus(pX);
        Matrix temp = matrixTimesVector(kalmanGain, vector);
        Matrix tempX = pX.plus(temp); // pXk+1 = pXk + kalmanGain*(yk - pXk)
        Matrix tempXC = pXC.plus(pNC.minus(kalmanGain.times(pXC.plus(pNC)))); // pXCk+1 = pXCk + pNCk - kalmanGain*(pXCk + pNCk)
        
        pX = tempX;
        pXC = tempXC;
        apX[i] = pX;
        apXC[i] = pXC;
        // I need the number of columns here = nr of samples! (right!?) YES
        for (int j = 0; j< pX.getColumnDimension(); j++){
            corrected[i][j+((k)*nrSH)] = pX.get(0,j);
            //System.out.println("tempX: " + tempX.get(0, j));
            //System.out.println("freq: " + corrected[i][j]);
        }
    }    
      
    public double[] copyArray(double[] in){
        double[] out = new double[in.length];
        for (int i =0; i < in.length; i++)
            out[i] = in[i];
        return out;
    }
    
    public double[][] copyArray(double[][] in){
        double[][] out = new double[in.length][in[0].length];
        for (int i = 0; i< in.length; i++){
            for (int j = 0; j< in[0].length; j++)
                out[i][j] = in[i][j];
        }
        return out;
    }    
    
    
        
    public Matrix initPX(double[][] s){        
        while (N*nrSH > s[0].length){
            N--;
        }
        int totalSampleSize = N*nrSH;
        Matrix meanT = new Matrix(1,totalSampleSize);
        double total = 0;
        int r = s.length;
        for (int j = 0; j < totalSampleSize; j++){
            for(int i= 0; i < r ; i++){
                total =+ s[i][j];
            }
            meanT.set(0,j,total/r);
            total = 0;
        }
        Matrix mean = new Matrix(1,nrSH);
        for(int i = 0; i < nrSH; i++){ 
            for(int j = 0; j < N ; j++){
                int a = j * nrSH + i;
                total =+ meanT.get(0, a);
            }
            mean.set(0,i,total/N);
            total =0;
         
        }
        return mean;
    }

    public Matrix initPX(double[] s){
        while (N*nrSH > s.length){
            N--;
        }
        double[] temp = new double[nrSH];
        for (int z = 0; z < nrSH; z++){
            temp[z] =0;
            for (int j = z; j < N* nrSH; j += nrSH ){
                temp[z] += s[j];
            }
            temp[z] = temp[z]/N;
        }

        return new Matrix(temp, 1);
    }

    public int calculateSH(double t){
        int hD =0; //heartbeatDuration in samples
        int sib = 0; //samples in between heartbeats
        boolean heartbeat = false;
        boolean hBfound = false;
        for(int i = 0; i < signals[0].length; i++){
            //System.out.println(signals[0][i]);
            if(!heartbeat && signals[0][i] > AMPLITUDE){
                if (hBfound)
                    break;
                heartbeat = true; 
            }               
            else if (heartbeat && signals[0][i] < AMPLITUDE){                
                heartbeat = false;
                hBfound = true;                
            }
            else if (heartbeat)
                hD++;
            else if (hBfound)
                sib++;
        }
        //System.out.println(hD + sib + " hd + sib");
        return (hD + sib);         
    }
    
    public Matrix getCovariance(Matrix m){
        int r = 1; //signals = 1
        int c = m.getColumnDimension(); // samples
        //System.out.println("r: " + r + "c: " + c);
        Matrix covarianceMatrix = new Matrix(c, c);
        
        //Calculate mean
        double totalM = 0;
        for(int sample = 0; sample < c; sample++){
            //System.out.println("signal: " + signal + "sample: " + sample);
            totalM += m.get(0, sample);
        }
        double avg = totalM/c;
        
        //Center the data. From every element, the mean of that channel is substracted.
        double[] result = new double[c];
        double substraction = avg;
        for (int i = 0; i < c; i++) {
            result[i] = m.get(0, i) - substraction;
        }

        //Calculate the covariance matrix V
        //Calculate the square of zeroMean
        double[][] res = new double[c][c];
        for(int i=0; i < c; i++){
            for(int s =0; s < c; s++){
                res[s][i] = result[i] * result[s];
            }
        }
        
        //Matrix squareZeroMean = new Matrix(res);
        //covarianceMatrix = squareZeroMean.times(1 / c);
        covarianceMatrix = new Matrix(res);
        return covarianceMatrix;
    }
    
    //m=horizontal vector
    public double getVariance(Matrix m){ 
        int c = m.getColumnDimension();
        double[] a = new double[c];       
        double totalM = 0;
        for(int i =0; i < c ; i++){
            totalM =+ m.get(0, i);
        }
        double mean = totalM/c;
        
        for(int i = 0; i < c; i++){
            double s = m.get(0,i);
            double d = Math.abs(s - mean);
            a[i] = d*d;
        }
        
        double totalV = 0;
        for(int i = 0; i< c; i++)
            totalV =+ a[i];
        double variance = totalV/c;
        return variance;
    }
    
    public double[][] getCorrected(){
        return corrected;
    }

    // Here: you can only put in a Matrix and a matrix object that actually is avector
    // so this method does a Matrix times vector multiplication!
    //checked with Matrix class: when you define a matrix as a vector it has 1 row and multiple columns!
    public Matrix matrixTimesVector(Matrix matrix, Matrix vector){
        double[][] matrixA = matrix.getArrayCopy();
        double[][] vectorA = vector.getArrayCopy();
        double[] temp = new double[matrixA[0].length];
        for (int i=0; i< temp.length; i++){
            temp[i] = 0;
        }
        for (int i=0; i< matrixA.length; i++){
            for (int j=0; j< matrixA[0].length; j++){
                temp[j] = temp[j] + matrixA[i][j] * vectorA[0][j];
            }
        }
        return new Matrix(temp, 1);
    }
}